/*!
  ******************************************************************************
  * @file           : guided.c
  * @author         :XYL
  * @createDate     : 2022/04/27
  * @lastEditors    :
  * @lastEditTime   : 2022/04/27
  * @brief          : 修复精度
  * 
  *     This file provides functions bellow to be called:
  * 
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2021 Zero_One NUAA.
  * All rights reserved.
  *
  ******************************************************************************
  */
 /* Includes ------------------------------------------------------------------*/
#include "main.hpp"
#include <math.h>



/**
     * 度换成弧度
     * @param  {Float} d  度
     * @return {[Float}   弧度
     */
double rad(double d)
{
	return d *toRAD;
}
    /**
     * 弧度换成度
     * @param  {Float} x 弧度
     * @return {Float}   度
     */
double deg(double x) 
{
	return x*toDEG;
}


    /**
     * 根据一个点的经纬度/方位角/距离得到另一个点的经纬度位置
     * @param  destination_lat   纬度
     *@param  destination_lng 经度
    *  @param  bearing  方位角
    *  @param  dist          距离
    *  @param   return caculate_latlng  经纬度
     */
target_latlng_t according_distance_caculate_latlng(float dist,float bearing,double destination_lat,double destination_lng)
{
//   printf("destination_lat = %d ,destination_lng = %d \n",destination_lat,destination_lng);
    target_latlng_t caculate_latlng;

    double a = 6378137;
    double b = 6356752.3142;
    double f = 1/298.257223563;

    double s = dist;
    double alpha1 = rad(bearing);
    double sinAlpha1 = sin(alpha1);
    double cosAlpha1 = cos(alpha1);

    double tanU1 = (1-f) * tan(rad(destination_lat));
    double cosU1 = 1 / sqrt((1 + tanU1*tanU1)), sinU1 = tanU1*cosU1;
    double sigma1 = atan2(tanU1, cosAlpha1);
    double sinAlpha = cosU1 * sinAlpha1;
    double cosSqAlpha = 1 - sinAlpha*sinAlpha;
    double uSq = cosSqAlpha * (a*a - b*b) / (b*b);
    double A = 1 + uSq/16384*(4096+uSq*(-768+uSq*(320-175*uSq)));
    double B = uSq/1024 * (256+uSq*(-128+uSq*(74-47*uSq)));

    double sigma = s / (b*A), sigmaP = 2*PI;
    double sinSigma ,cosSigma,cos2SigmaM;
    while (fabs(sigma-sigmaP) > 1e-12) {
            cos2SigmaM = cos(2*sigma1 + sigma);
            sinSigma = sin(sigma);
            cosSigma = cos(sigma);
        double deltaSigma = B*sinSigma*(cos2SigmaM+B/4*(cosSigma*(-1+2*cos2SigmaM*cos2SigmaM)-
            B/6*cos2SigmaM*(-3+4*sinSigma*sinSigma)*(-3+4*cos2SigmaM*cos2SigmaM)));
        sigmaP = sigma;
        sigma = s / (b*A) + deltaSigma;
    }

    double tmp = sinU1*sinSigma - cosU1*cosSigma*cosAlpha1;
    double lat2 = atan2(sinU1*cosSigma + cosU1*sinSigma*cosAlpha1,
        (1-f)*sqrt(sinAlpha*sinAlpha + tmp*tmp));
    double lambda = atan2(sinSigma*sinAlpha1, cosU1*cosSigma - sinU1*sinSigma*cosAlpha1);
    double C = f/16*cosSqAlpha*(4+f*(4-3*cosSqAlpha));
    double L = lambda - (1-C) * f * sinAlpha * (sigma + C*sinSigma*(cos2SigmaM+C*cosSigma*(-1+2*cos2SigmaM*cos2SigmaM)));

    double revAz = atan2(sinAlpha, -tmp);  // final bearing
    // float moveLongitude = (destination_lng*0.0000001)+deg(L);
    // float moveLatitude = deg(destination_lat*0.0000001);

    caculate_latlng.target_lat = deg(lat2);
    caculate_latlng.target_lng = destination_lng+deg(L);
    //   printf("deg = %f,ddeg = %f \n", deg(lat2),deg(L));

    return caculate_latlng;
    // return {long:lon1+u.deg(L),lati:u.deg(lat2)};

}

    /**
     * 根据两个点的经纬度，求解方位角
     * @param  destination_lat   其中一点纬度
     *@param  destination_lng   其中一点经度
        @param  origin_lat                  另一个点纬度
        @param  origin_lng                 另一个点经度
    *  @param return                       方位角
        */
    double according_latlng_caculate_bearing(int32_t origin_lat,int32_t origin_lng,int32_t destination_lat,int32_t destination_lng){
        
    double Rc=6378137;  
    double Rj=6356725;

    double Ec=Rj+(Rc-Rj)*(90-(origin_lat*0.0000001))/90;
    double Ed= Ec * cos((origin_lat*0.0000001)*PI/180);

    double dx=((destination_lng*0.0000001)*PI/180-(origin_lng*0.0000001)*PI/180)*Ed;
    double dy=((destination_lat*0.0000001)*PI/180-(origin_lat*0.0000001)*PI/180)*Ec;
    double angle=0.0;
    angle=atan(abs(dx/dy))*180/PI;
    double dLo=(destination_lng*0.0000001)-(origin_lng*0.0000001);
    double dLa=(destination_lat*0.0000001)-(origin_lat*0.0000001);
    if(dLo>0&&dLa<=0)
        {  
            angle=(90-angle)+90;
        }  
        else if(dLo<=0&&dLa<0)
        {  
            angle=angle+180;
        }else if(dLo<0&&dLa>=0)
        {  
            angle= (90-angle)+270;
        }  

        return angle;
 }


   /**
     * 求解两个点的相对位置
     * @param  LonA   原点经度
     *@param  LonB   目标经度
     @param  LatA   原点纬度
     @param  LatB   目标纬度
     返回的x为正北方向，y为正东方向
     */
 target_pos_t get_vector_xy_from_current_position_NE_double(double LonA,double  LonB,double  LatA,double   LatB){
    double LonA1=rad(LonA);
    double LonB1=rad(LonB);
    double LatA1=rad(LatA);
    double LatB1=rad(LatB);
    target_pos_t target_pos;

    double C = sin(LatA1)*sin(LatB1) + cos(LatA1)*cos(LatB1)*(cos(LonA1-LonB1));
    //  double D = 6378137.0*acos(fminl(fmaxl(C,-1.0),1.0));
    target_pos.distance = 6378137.0*acos(C);

    double error = LonA1 - LonB1;

    double temp_y = sin(LonB1-LonA1)*cos(LatB1);
    double temp_x = cos(LatA1)*sin(LatB1)-sin(LatA1)*cos(LatB1)*cos(LonB1- LonA1);
    target_pos.target_yaw = atan2(temp_y,temp_x);

    target_pos.target_pos_x = target_pos.distance * cos(target_pos.target_yaw);
    target_pos.target_pos_y = target_pos.distance * sin(target_pos.target_yaw);

    return target_pos;
}
     /**
     * 求解两个点的相对位置
     *  x指向正北
     */
// target_pos_t get_vector_xy_from_current_position_NE_double(double LonA,double  LonB,double  LatA,double   LatB)
// {
//     target_pos_t target_pos;
//     target_pos.target_pos_x = (LatB - LatA) * toRAD * 111318.9 * toDEG;
//     target_pos.target_pos_y = (LonB - LonA) * toRAD * 111318.9 *0.848* toDEG;  
//     return target_pos;
// }